#ifndef FPGA_TIMER
#define FPGA_TIMER
#endif

#undef OLD_FPGA_DRIVER

#include <sched.h>
#include <stdlib.h>
#include <time.h>
#include <signal.h>
#include <sys/time.h>
#include "hwFpga.h"
#include "hdmacros.h"
#include "eph/thread.h"

#include <getopt.h>
#include <unistd.h>
#include <time.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <hwcore.h>
#include <semaphore.h>
#include "hwSwitch.h"
#include "sample_cha.h"
#include "core_match.h"

#define TASK_ISOLATION_SYNC     1<<1
#ifndef PR_TASK_ISOLATION   /* Not in system headers yet? */
# define PR_TASK_ISOLATION      48
# define PR_TASK_ISOLATION_ENABLE   (1 << 0)
# define PR_TASK_ISOLATION_SET_SIG(sig) (((sig) & 0x7f) << 8)
# define PR_TASK_ISOLATION_GET_SIG(bits) (((bits) >> 8) & 0x7f)
#endif


#define timeout_init(tv) gettimeofday((tv), NULL)
static int timeout(struct timeval *start, double secs)
{
    struct timeval tv;
    double time;

    gettimeofday(&tv, NULL);
    time = (tv.tv_sec - start->tv_sec) +
        (tv.tv_usec - start->tv_usec) / 1000000.0;
    return time >= secs;
}

static inline int prctl_isolation(int flags)
{
    return prctl(PR_TASK_ISOLATION, flags, 0, 0, 0);
}

static void set_task_isolation(int flags)
{
    int rc;
    struct timeval start;

    /* Wait for up to a second for the kernel to isolate this core. */
    timeout_init(&start);
    do {
        rc = prctl_isolation(flags);
        if (rc == 0 || errno != EAGAIN)
            break;
    } while (!timeout(&start, 1));
    if (rc != 0) {
        prctl_isolation(0);
        printf("couldn't enable isolation (%d): FAIL\n", errno);
    }
}


void sigint_handler(int sig) {
    printf("\nCaught SIGINT (Ctrl+C)! Exiting gracefully...\n");
    // You can clean up resources here if necessary before exiting
    fpga_close_all();
    exit(0);  // Exit the program after handling the signal
}


static int interval;
static int loopCnt;


#define half 500
int deltaTable[2*half] = {0};

void printfDelta() {
    for(int i=0; i<2*half; i++){
        if(deltaTable[i] == 0)
            continue;
        printf("%.3f, %d\n", 25+(i-half)*0.001, deltaTable[i]);
    }
}

// length must be multiplier of 128b/16B

static inline void pre_test(fpgaIO_t *fi)
{
    uint64_t cnt[2]={1,1};
    uint64_t cnt_prv[2]={1,1};

    cnt[0] = fpga_get_seq_no(fi, 0);
    cnt[1] = fpga_get_seq_no(fi, 1);
    cnt_prv[0] = cnt[0];
    cnt_prv[1] = cnt[1];
    int chan = cnt_prv[0]>cnt_prv[1]?1:0;

    uint64_t loop=0;
    while(1) {
        if (loop>120000)
            break;
        cnt[0] = fpga_get_seq_no(fi, 0);
        cnt[1] = fpga_get_seq_no(fi, 1);
        if (cnt[0]==cnt_prv[0] && cnt[1]==cnt_prv[1])
            continue;
        ++loop;
        cnt_prv[0] = cnt[0];
        cnt_prv[1] = cnt[1];
        chan = 1-chan;
    }
}

static inline void fpga_counter_test(fpgaIO_t *fi)
{
    uint64_t cnt[2]={1,1};
    uint64_t cnt_prv[2]={1,1};
    unsigned long long tsc1 = 0;
    unsigned long long tsc2 = 0;
    int interval_ns = interval * 1000;

    cnt[0] = fpga_get_seq_no(fi, 0);
    cnt[1] = fpga_get_seq_no(fi, 1);
    cnt_prv[0] = cnt[0];
    cnt_prv[1] = cnt[1];
//    fprintf(stderr, "    fpgaCnt::     cnt[0]=%lu,     cnt[1]=%lu\tdelta=%lu\t%llu\n",
//		cnt[0], cnt[1], max(cnt[0],cnt[1])-min(cnt[0],cnt[1]), (tsc2-tsc1)/3600);

    uint64_t loop=0;
   // busy_loop(1);
   // set_task_isolation(PR_TASK_ISOLATION_ENABLE);
    while (cnt[0]==cnt_prv[0] && cnt[1]==cnt_prv[1]){
        cnt[0] = fpga_get_seq_no(fi, 0);
        cnt[1] = fpga_get_seq_no(fi, 1);
    }
    tsc1 = get_cycles(); 
    cnt_prv[0] = cnt[0];
    cnt_prv[1] = cnt[1];
    while(1) {
        if (loop>loopCnt)
            break;
        cnt[0] = fpga_get_seq_no(fi, 0);
        cnt[1] = fpga_get_seq_no(fi, 1);
        if (cnt[0]==cnt_prv[0] && cnt[1]==cnt_prv[1])
            continue;
        tsc2 = get_cycles();
	   // if ((max(cnt[0],cnt[1])-min(cnt[0],cnt[1])) != 1) {
	   //     fprintf(stderr, "    jumpy counter\n");
	   //     fprintf(stderr, "    fpgaCnt::     cnt[0]=%lu,     cnt[1]=%lu\tdelta=%lu\t%llu\n",
	   // 	    cnt[0], cnt[1], max(cnt[0],cnt[1])-min(cnt[0],cnt[1]), (tsc2-tsc1)/3600);
	   //     fprintf(stderr, "    fpgaCnt::prev_cnt[0]=%lu,prev_cnt[1]=%lu\n",
	   // 	    cnt_prv[0], cnt_prv[1]);
	   //     break;
	   // 
	   // }
        ++loop;
        cnt_prv[0] = cnt[0];
        cnt_prv[1] = cnt[1];
        int delta_ns = (tsc2-tsc1)/3.6;
        int jitter_ns = delta_ns - interval_ns;
        if (jitter_ns >= half || jitter_ns < (-1*half)) {
            fprintf(stderr, "jitter greater than 5us : %d us\n", jitter_ns);
            break;
        }else{
	       // fprintf(stderr, "    fpgaCnt::     cnt[0]=%lu,     cnt[1]=%lu\tdelta=%lu\t%d, %d\n",
           //         cnt[0], cnt[1], max(cnt[0],cnt[1])-min(cnt[0],cnt[1]), delta_ns, jitter_ns);
            ++deltaTable[half+jitter_ns];
        }
        tsc1 = tsc2;
    }
   // prctl_isolation(0);
    printfDelta();
}


int main(int argc, char *argv[])
{
    fpgaIO_t *fi;

    if (argc != 4) {
        printf("Usage: %s <period_in_us> <dma_length> <duration_sec>\n", argv[0]);
        exit(EXIT_SUCCESS);
    }
    interval    = atoi(argv[1]);
    int dma_len     = atoi(argv[2]);
    int duration = atoi(argv[3]);
    loopCnt = duration * 40000;

    if (signal(SIGINT, sigint_handler) == SIG_ERR) {
        perror("Unable to catch SIGINT");
        return 1;
    }

    EPRT_FIFO();
    EPCPU_BIND(3);
    int numFpga;

    // 1) init to discovery fpga
    numFpga = fpga_init();
    printf("number of FPGAs:%d\n", numFpga);
    if (numFpga == 0)
        exit(EXIT_SUCCESS);

    // 2) get FPGA instance
    if (numFpga == 1) {
        fi = fpga_instance_by_idx(0);
    } else {
        int idx = 999;
        do {
            for (int ii=0; ii<numFpga; ++ii) {
                fi = fpga_instance_by_idx(ii);
                printf("%d : \n", ii);
                fpga_print(fi);
            }
            printf("Choose FPGA(0-%d):", numFpga-1);
            scanf("%d", &idx);
        } while (idx >= numFpga);
        fi = fpga_instance_by_idx(idx);
    }
    // 3) config FPGA
    FPGAIO_SET_MASTER(fi);
    FPGAIO_SET_DMA_IN_US(interval);
    FPGAIO_SET_C2F_LEN(fi, dma_len);
    FPGAIO_SET_F2C_LEN(fi, dma_len);

    // 4) open fpga
                fpga_print(fi);
    int ret = fpga_open(fi);
    if (ret != 0) {
        printf("open fpga failed\n");
        return -1;
    }

    // 5) init f2c/c2f, model operations
    _fpga_init_c2f(fi);
    _fpga_init_f2c(fi);

    // 6) start FPGA
    fpga_start(fi);
    //_fpga_print_f2c_dma(fi, -1); 
    
   program_cha_counters();
   sample_cha_counters();
   //pre_test
   pre_test(fi);
   sample_cha_counters();
   SlicePair slices = find_two_large_indices();
   int cpu = match_core(slices.idx1, slices.idx2, 1); 
   EPCPU_BIND(cpu);
   busy_loop(1); 
    // 7) test FPGA Counter
    printf("================= Counter Test =====================\n");
    fpga_counter_test(fi);
    printf("================= Counter Test =====================\n");
    /*
    _fpga_print_f2c_dma(fi, -1); 
*/
    
    // 8) close fpga
    fpga_close_all();

    return 0;
}

